#include "fsm.h"

#include "fsm/arm/working_state.h"
#include "fsm/arm/reset_state.h"
#include "fsm/arm/sleep_state.h"
#include "fsm/arm/pause_state.h"
#include "fsm/arm/prepare_state.h"

//#include "remotecontroller.h"
#include "base_state.h"
#include "masters/arm_controller.h"
using namespace std;
using namespace arm;
//FSM::FSM(): slave_px0(0),slave_py0(0),slave_pz0(0),master_px0(0.4),master_py0(-0.25),master_pz0(-0.4)
//{
//    device_=nullptr;

//    force_state_=new ForceState();
//    position_state_=new PositionState();
//    idle_state_=new IdleState();
//    ready_state_=new ReadyState();
//    pause_state_=new PauseState();
//    cur_state_=idle_state_;
//}
FSM::FSM(ArmController *arm_controller)
{
    arm_controller_=arm_controller;

    sleep_state_=new SleepState();
    prepare_state_=new PrepareState();
    reset_state_=new ResetState();
    working_state_=new WorkingState();
    pause_state_=new PauseState();
    cur_state_=sleep_state_;
    cur_state_->OnEntry(this);
}

FSM::~FSM()
{
    //delete device_;

    delete sleep_state_;
    delete prepare_state_;
    delete reset_state_;
    delete working_state_;
    delete pause_state_;
}

void FSM::SetState(BaseState::StateType state)
{
    //ForceMode,PositionMode,IdleMode,ReadyMode,PauseMode
    if(cur_state_->GetState()==state)
        return;
    switch (state) {
    case BaseState::kResetState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=reset_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kWorkingState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=working_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kSleepState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=sleep_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kPrepareState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=prepare_state_;
        cur_state_->OnEntry(this);
        break;
    case BaseState::kPauseState:
        last_state_=cur_state_;
        cur_state_->OnExit(this);
        cur_state_=pause_state_;
        cur_state_->OnEntry(this);
        break;
    default:
        cout<<"unkown state"<<endl;
        break;
    }

}





//bool FSM::SetSlaveTargetPos(const double target_px, const double target_py, const double target_pz)
//{
//    remote_controller_->SendTargetPos2Robot(target_px,target_py,target_pz);
//    return true;
//}

//bool FSM::SetMasterTargetPos(const double target_px, const double target_py, const double target_pz)
//{
//    master_target_pos[0]=target_px;
//    master_target_pos[1]=target_py;
//    master_target_pos[2]=target_pz;
//    return true;
//}



void FSM::Update()
{
    //string states[]={"ForceState","PositionState","IdleState","ReadyState","PauseState"};
    cur_state_->Update(this);
    //cout<<"cur_state:"<<cur_state_->GetState()<<endl;
}
